#include <ros/ros.h>
#include <std_msgs/String.h>
#include "ultra_band_wave_ladar/Sensor_mmRad.hpp"
#include "ultra_band_wave_ladar/Sensor_Ultra.hpp"
#include "ultra_band_wave_ladar/Sensor_RadLogic.hpp"
//#include "aics_motion/Sensor_mmRad_info.h"
//#include "aics_motion/Sensor_UltraRad_info.h"
//#include "aics_motion/Machine_status.h"

void Listen_StatusCallback_Machine(const std_msgs::String::ConstPtr& msg)
{
  //处理毫米波收到的小车运行状态
}

void Listen_CommdCallback_mmRad(const std_msgs::String::ConstPtr& msg)
{
  //处理毫米波收到的传感器扫描数据
}

void Listen_CommdCallback_UltraRad_back(const std_msgs::String::ConstPtr& msg)
{
  //处理超声波收到的传感器扫描数据
}

void Listen_CommdCallback_UltraRad_left(const std_msgs::String::ConstPtr& msg)
{
  //处理超声波收到的传感器扫描数据
}

void Listen_CommdCallback_UltraRad_right(const std_msgs::String::ConstPtr& msg)
{
  //处理超声波收到的传感器扫描数据
}

// void Listen_StatusCallback_Machine(const aics_motion::Machine_status::ConstPtr& msg)
//{
//处理毫米波收到的小车运行状态
// sensorinfo.machine_st = msg->Machine_st;
// sensorinfo.run_st = msg->run_status;
// sensorinfo.array_portnames=msg->port_names;
// ROS_ERROR_STREAM("Recive Machine's status: %d", sensorinfo.machine_st);
// ROS_ERROR_STREAM("Recive Machine's running status: %d", sensorinfo.run_st);
// ROS_ERROR_STREAM("Recive Sensor ports number: %d ", msg->portnames.size());
// ROS_ERROR_STREAM("Recive Sensor ports number:");
// for (int i = 0; i < msg->portnames.size(); i++)
//{
// ROS_ERROR_STREAM("%s ", msg->portnames[i]);
//}
//}

// void Listen_CommdCallback_mmRad(const aics_motion::Sensor_mmRad_info::ConstPtr& msg)
//{
//处理毫米波收到的传感器扫描数据
// sensorinfo.mmRad_front= msg->ScanData;
// ROS_ERROR_STREAM("Recive mmRad scan Data size: %d ", msg->ScanData.size());
// ROS_ERROR_STREAM("Recive mmRad's scan Data:");
// for (int i = 0; i < msg->ScanData.size(); i++)
//{
// ROS_ERROR_STREAM("%d ", msg->ScanData[i]);
//}
//}

// void Listen_CommdCallback_UltraRad_back(const aics_motion::Sensor_UltraRad_info::ConstPtr& msg)
//{
//处理超声波收到的传感器扫描数据
// sensorinfo.array_UltraRad_back = msg->ScanData;
// ROS_ERROR_STREAM("Recive back UltraRad scan Data size: %d ", msg->ScanData.size());
// ROS_ERROR_STREAM("Recive back UltraRad's scan Data:");
// for (int i = 0; i < msg->ScanData.size(); i++)
//{
// ROS_ERROR_STREAM("%d ", msg->ScanData[i]);
//}
//}

// void Listen_CommdCallback_UltraRad_left(const aics_motion::Sensor_UltraRad_info::ConstPtr& msg)
//{
//处理超声波收到的传感器扫描数据
// sensorinfo.array_UltraRad_right = msg->ScanData;
// ROS_ERROR_STREAM("Recive left UltraRad scan Data size: %d ", msg->ScanData.size());
// ROS_ERROR_STREAM("Recive left UltraRad's scan Data:");
// for (int i = 0; i < msg->ScanData.size(); i++)
//{
// ROS_ERROR_STREAM("%d ", msg->ScanData[i]);
//}
//}

// void Listen_CommdCallback_UltraRad_right(const aics_motion::Sensor_UltraRad_info::ConstPtr& msg)
//{
//处理超声波收到的传感器扫描数据
// sensorinfo.array_UltraRad_right = msg->ScanData;
// ROS_ERROR_STREAM("Recive right UltraRad scan Data size: %d ", msg->ScanData.size());
// ROS_ERROR_STREAM("Recive right UltraRad's scan Data:");
// for (int i = 0; i < msg->ScanData.size(); i++)
//{
// ROS_ERROR_STREAM("%d ", msg->ScanData[i]);
//}
//}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "Radsensor listen from motion");
  ros::NodeHandle nh;

  ros::Subscriber sub_Machine;
  sub_Machine = nh.subscribe("Machine_Status", 1000, Listen_StatusCallback_Machine);

  ros::Subscriber sub_mmRad;
  sub_mmRad = nh.subscribe("mmRadSensor_ScanData", 1000, Listen_CommdCallback_mmRad);

  ros::Subscriber sub_UltraRad_back;
  sub_UltraRad_back = nh.subscribe("UltraRadSensor_back_ScanData", 1000, Listen_CommdCallback_UltraRad_back);

  ros::Subscriber sub_UltraRad_left;
  sub_UltraRad_left = nh.subscribe("UltraRadSensor_left_ScanData", 1000, Listen_CommdCallback_UltraRad_left);

  ros::Subscriber sub_UltraRad_right;
  sub_UltraRad_right = nh.subscribe("UltraRadSensor_right_ScanData", 1000, Listen_CommdCallback_UltraRad_right);

  ros::spin();

  return 0;
}